package kit.edu.lego.gruppe3.levels.line;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.sensors.Scanner;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class findObstacle implements Behavior {

	private DifferentialPilot robot = ComponentsRepository.getInstance()
			.getRobotPilot();

	private int minDistance = 14;
	private int maxDistance = 20;

	private boolean distanceToClose = false;
	private boolean distanceToFar = false;

	private int distance;

	@Override
	public void action()
	{
		if (distanceToClose)
		{
			robot.setTravelSpeed(5);
			robot.steer(60);
			while (robot.isMoving())
			{
				if (Scanner.getRangeAt(90, true) >= 14)return;
				if (ComponentsRepository.getInstance().getLightSensor().readValue() > ComponentsRepository.LIGHT_TRASHOLD)
				{
					robot.stop();
					ComponentsRepository.evadeObstacle = false;
					return;
				}
			}
		}
		if (distanceToFar)
		{
			robot.setTravelSpeed(5);
			robot.steer(-60);

			while (robot.isMoving())
			{
				if (Scanner.getRangeAt(90, true) < 20) return;
				if (ComponentsRepository.getInstance().getLeftTouchSensor().isPressed()
					|| ComponentsRepository.getInstance().getRightTouchSensor().isPressed())
				{
					robot.stop();
					robot.rotate(20);
					return;
				}
				if (ComponentsRepository.getInstance().getLightSensor().readValue() > ComponentsRepository.LIGHT_TRASHOLD)
				{
					robot.stop();
					ComponentsRepository.evadeObstacle = false;
					robot.rotate(50);
					return;
				}
			}
		}
	}

	@Override
	public void suppress()
	{

	}

	@Override
	public boolean takeControl()
	{
		distance = ComponentsRepository.getInstance().getSonicSensor()
				.getDistance();
		distanceToClose = minDistance > distance;
		distanceToFar = maxDistance < distance;
		return LevelSelector.getInstance().isLine()
				&& ComponentsRepository.evadeObstacle
				&& (distanceToClose || distanceToFar);
	}

}
